#include "TestMemoryPool.h"
#ifndef CFG_IGNORE_TESTMEMORYPOOL

#define MAX_ACHUNKS 4		//511
#define MAX_BCHUNKS 2		//31
#define MIN(a,b) a < b ? a : b
MemoryPoolSystem::MemoryPoolSystem(bool automatic_pool_generation, unsigned int default_chunk_count, unsigned char default_chunk_size)
{
	m_automatic_pool_generation = automatic_pool_generation;
	m_default_chunk_count = default_chunk_count;
	m_default_chunk_size = default_chunk_size;
	m_pfirst = NULL;

	AddPool(GenerateNewPool(default_chunk_count, default_chunk_size));
}
MemoryPool* MemoryPoolSystem::GenerateNewPool(unsigned int chunk_count, unsigned char chunk_size)
{
	MemoryPool * pool = (MemoryPool*)malloc(sizeof(MemoryPool));
	size_t sze = (chunk_count * chunk_size) + (chunk_count * sizeof(ushort));
	pool->m_chunkinfo_start = (unsigned char*)malloc(sze);
	pool->m_pool_start = pool->m_chunkinfo_start + chunk_count * sizeof(ushort);
	memset(pool->m_chunkinfo_start, 0, sizeof(ushort) * chunk_count);
	pool->m_chunk_size = chunk_size;
	pool->m_chunks = chunk_count;
	pool->m_pnext = NULL;
	pool->SetUpChunkInfo();
	return pool;
}
void MemoryPoolSystem::AddPool(MemoryPool * pool)
{
	if(m_pfirst == NULL)
	{
		m_pfirst = pool;
		return;
	}
	MemoryPool * pslot = m_pfirst;
	while(pslot->m_pnext != NULL)
	{
		pslot = pslot->m_pnext;
	}
	pslot->m_pnext = pool;
}
void MemoryPoolSystem::RemovePool(MemoryPool * pool)
{
	free(pool->m_chunkinfo_start);
	free(pool);
	/*if(m_pfirst == pool)
	{
		m_pfirst = pool->m_pnext;
	}
	MemoryPool * p = m_pfirst;
	while(p != NULL)
	{
		if(p == pool)
		{

			return;
		}
		p = p->m_pnext;
	} */
}
void MemoryPoolSystem::RemoveAllPools()
{
	MemoryPool* pool = m_pfirst;
	while(pool != NULL)
	{
		free(pool->m_chunkinfo_start);
		MemoryPool* for_removal = pool;
		pool = pool->m_pnext;
		free(for_removal);
	}
	m_pfirst = NULL;
}

void* MemoryPoolSystem::Allocate(unsigned int size)
{
	if(size == 0)
	{
		assert(0 && "Invalid allocation request!");
		size = 1;
	}
	MemoryPool * pool = m_pfirst; void* alloc = NULL;
	while(pool != NULL)
	{
		alloc = pool->Allocate(size);
		if(alloc != NULL)
		{
			return alloc;
		}
		pool = pool->m_pnext;
	}
	assert(0 && "There was no pool which can evaluate your request. Check if memory pool is enough big and if auto pool generation is enabled!");
	return NULL;
}
void  MemoryPoolSystem::Deallocate(void* chunk)
{
	//this time memorypool sys must find the pool which has done the allocation...
	
	MemoryPool * pool = m_pfirst;
	while(pool != NULL)
	{
		if(pool->m_pool_start <= chunk && (pool->m_pool_start + pool->m_chunks * pool->m_chunk_size) >= chunk)
		{
			//pool->Deallocate(chunk);
			break;
		}
		pool = pool->m_pnext;
	}
	//*/
}

//---------------------------------------------------------------------------------
//The actual implementation of allocate and delocate
//---------------------------------------------------------------------------------
void MemoryPool::SetUpChunkInfo()
{
	//this generates full chunk info points, Allocate is based on them to find free memory
	size_t free_chunks = m_chunks - 1;
	ushort * pnow = &*reinterpret_cast<ushort*>(m_chunkinfo_start);
	ushort chunks_ = 0;
	while(free_chunks != 0)
	{
		chunks_ = MIN(MAX_ACHUNKS, free_chunks);
		free_chunks -= chunks_;
		*pnow = SetCertainBits(14,2,chunks_,*pnow);
		pnow += chunks_;
	}
	m_chunks_used = 0;
	m_next_alloc[0] = reinterpret_cast<ushort*>(m_chunkinfo_start);
	m_next_alloc[1] = m_next_alloc[0];
}
void* MemoryPool::Allocate(unsigned int size)
{ 
	ushort* last_apoint; // used in the SetBlockAllocated.
	uchar* alloc = FindFreeBlock(size, &last_apoint);
	if(alloc == NULL)
	{
		assert(0 && "Could not find a suitable block for the requested size!");
		return NULL;
	}
	if(!SetBlockAllocated(last_apoint, alloc, size))
	{
		assert(0 && "Could not allocate the found block.");
		return NULL;
	}
	return GetDataPtr(alloc);
}


uchar* MemoryPool::FindFreeBlock(size_t size, ushort** last_apoint)
{
	//TODO. If I'm gonna support multipl pools at the same time, 
	//I need faster way to track the used and not used memory to choose the most suitable pool for the request
	uchar* alloc = FindFreeBlockFrom(reinterpret_cast<uchar*>(m_next_alloc[1]), size, last_apoint);
	if(alloc == NULL)
	{
		alloc = FindFreeBlockFrom(reinterpret_cast<uchar*>(m_chunkinfo_start), size, last_apoint);
	}
	return alloc;
}
uchar* MemoryPool::FindFreeBlockFrom(uchar* pfrom, size_t size, ushort** last_apoint)
{
	//variables
	ushort * pnow = &*reinterpret_cast<ushort*>(pfrom); // chunk info iterator
	ushort * pend = &*reinterpret_cast<ushort*>(m_pool_start - 2);
	ushort   next_step = 0;
	ushort * possible_alloc = pnow; // read the var name
	*last_apoint = pnow;
	size_t chunks_required = size / m_chunk_size;
	if(size % m_chunk_size != 0)
	{
		++chunks_required;
	}
	size_t chunk_chain = 0; // how many chunks we have collected?
	while(pnow != pend)
	{
		next_step = GetCertainBits(9, 7, *pnow);
		if(GetCertainBits(1,1,*pnow) == 0) // is this chunk beeing used?
		{
			//no it is free. Lets check if the there are enough chunks free after it
			chunk_chain += next_step;
			if(chunk_chain >= chunks_required)
			{
				//there is enoug space, return the pointer
				return &(*reinterpret_cast<uchar*>(possible_alloc));
			}
		}else
		{
			//it is not free. chunk_chain is ended. zero it out!
			chunk_chain = 0;
			*last_apoint = pnow;
			if(pnow + 1 < pend)
				possible_alloc += GetCertainBits(9,7,*pnow);
		}
		pnow = pnow + next_step;
	}
	return NULL;
}
bool MemoryPool::SetBlockFree(uchar* block)
{
	return NULL;
}
bool MemoryPool::SetBlockAllocated(ushort* last_apoint, uchar* block, size_t size)
{
	ushort chunks = (size / m_chunk_size);
	if(size % m_chunk_size != 0)
	{
		++chunks;
	}
	if(!SetBlockAPointsAllocated(last_apoint, block, chunks))
	{
		return false;
	}
	if(!SetBlockBPointsAllocated(block, chunks))
	{
		return false;
	}
	return true;
}

bool MemoryPool::SetBlockAPointsAllocated(ushort* last_apoint, uchar* block, size_t chunks)
{
	m_chunks_used+=chunks;
	ushort* iter = &(*reinterpret_cast<ushort*>(block));
	ushort* end  = &(*reinterpret_cast<ushort*>(iter+chunks)); // this in most cases would be free
	ushort* pool_end = &(*reinterpret_cast<ushort*>(m_pool_start - 2));
	//check if end is used. if so then the end is not the above
	if(GetCertainBits(1,1,*end) == 1)
	{
		//trace the the real end
		while(GetCertainBits(1,1,*end) != 0 && end != pool_end)
		{
			end += GetCertainBits(9,7,*end);
		}
	}
	iter = last_apoint; // we need to continue the last A checkpoint. It shoud be passed from 
	ushort  chunks_left = end - iter;
	ushort* original_aend = 0;
	//trace the first a point after the end, so I can continuew the chain after I set the block
	while(iter + GetCertainBits(9,7,*iter) <= end)
	{
		iter += GetCertainBits(9,7,*iter);
	}
	iter += GetCertainBits(9,7,*iter);
	original_aend = iter;
	iter = last_apoint;
	ushort  chunks_proc = 0;
	*iter = SetCertainBits(1,1,1,*iter); 
	while(iter != end)
	{
		chunks_proc = MIN(chunks_left, MAX_ACHUNKS);
		chunks_left -= chunks_proc;
		*iter = SetCertainBits(9, 7, chunks_proc, *iter);
		*iter = SetCertainBits(1,1,1,*iter);
		iter += chunks_proc;
	}
	//now we need to match the new first free chunk to the next(create a new apoint if required)
	if(original_aend - iter >= 0)
	{
		*iter = SetCertainBits(9, 7 , original_aend - iter, *iter);
	}
	m_next_alloc[1] = m_next_alloc[0];
	m_next_alloc[0] = iter;
	return true;
}
bool MemoryPool::SetBlockBPointsAllocated(uchar* block, size_t chunks)
{
	ushort* now = &(*reinterpret_cast<ushort*>(block));
	ushort temp = 0;
	--chunks;
	while(chunks != 0)
	{
		temp = MIN(chunks, MAX_BCHUNKS);
		chunks -= temp;
		*now = SetCertainBits(5, 2, temp, *now);
		now += temp;
	}
	//this can be done differently...
	*now = SetCertainBits(5,2,0,*now);
	return true;
}




uchar* MemoryPool::GetDataPtr(uchar * info)
{
	return m_pool_start + (info - m_chunkinfo_start) * m_chunk_size / 2;
}
uchar* MemoryPool::GetInfoPtr(uchar * data)
{
	return m_chunkinfo_start + (data - m_pool_start) / m_chunk_size * 2;
}
ushort* MemoryPool::FindAPointByCPoint(ushort* cpoint)
{
	ushort* iter = &(*reinterpret_cast<ushort*>(m_chunkinfo_start));
	ushort* end  = &(*reinterpret_cast<ushort*>(m_pool_start - 2));
	ushort chunks_a = 0;
	while(iter != end)
	{
		chunks_a = GetCertainBits(9,7,*iter);
		if(cpoint <= iter + chunks_a - 1)
		{
			if(cpoint >= iter)// not really sure if this is needed... but never it will stay
			{
				return iter;
			}
		}
		iter += chunks_a;
	}
	return NULL; // invalid C point
}

void MemoryPool::Deallocate(void* chunk)
{
	ushort* pnow = &(*reinterpret_cast<ushort*>(chunk));
	ushort chunks_count = GetCertainBits(5,2,*pnow);
	//while(chunks_count != 0)
	{
		pnow += chunks_count;
		chunks_count = GetCertainBits(5,2, *pnow);
	}
}
unsigned int MemoryPool::GetUsedBytes()
{
	return m_chunk_size * m_chunks_used;
}

void MemoryPool::PrintChunkInfo()
{
	ushort* start = &(*reinterpret_cast<ushort*>(m_chunkinfo_start));
	ushort* end =  &(*reinterpret_cast<ushort*>(m_pool_start));
	ushort* iter = start;
	std::cout << "Chunk\tAlloc\tUsed\tBFlag\tPFlag\tChunkAdr\tAdress" << std::endl;
	while(iter != end)
	{
		std::cout << (iter - start) << ":\t" << GetCertainBits(1,0,*iter) << "\t" << GetCertainBits(1,1,*iter) << "\t" << GetCertainBits(5,2,*iter) << "\t" << GetCertainBits(9,7,*iter) << "\t" << iter << "\t" << reinterpret_cast<ushort*>(GetDataPtr(reinterpret_cast<uchar*>(iter))) << std::endl;
		iter+=1;
	}
}
ushort MemoryPool::SetCertainBits(uchar bit_count, uchar start, ushort value, ushort source)
{
	value = value << (16 - start - bit_count); // prepare the bits, by putting them in the right place accordign to source
	ushort mask = 1; // create the mask with initial value of 1
	mask = (mask << bit_count) - 1; // now get a bigger number so we can generate enough big number. Then substract by 1. (1000 - 1 = 0111)->3 bits mask
	mask = mask << (16 - start - bit_count); // move them to the right place according to the source
	mask = ~mask; // inverse
	source = source & mask; // clear the bits for writing
	source = source | value; // set the value
	return source;
} 
size_t MemoryPool::SetCertainBits(uchar bit_count, uchar start, size_t value, size_t source)
{
	value = value << (32 - start - bit_count); // prepare the bits, by putting them in the right place accordign to source
	size_t mask = 1; // create the mask with initial value of 1
	mask = (mask << bit_count) - 1; // now get a bigger number so we can generate enough big number. Then substract by 1. (1000 - 1 = 0111)->3 bits mask
	mask = mask << (32 - start - bit_count); // move them to the right place according to the source
	mask = ~mask; // inverse
	source = source & mask; // clear the bits for writing
	source = source | value; // set the value
	return source;
}
ushort MemoryPool::GetCertainBits(uchar bit_count, uchar start, ushort source)
{
	ushort mask = 1; // create a mask with initial of 1
	mask = (mask << bit_count) - 1; // generate enough 1's which will filter the data
	mask = mask << (16 - start - bit_count); // move the mast to the data we need
	source = source & mask; // apply the mast
	source = source >> (16 - start - bit_count); // move the bits left to the beginning for return
	return source;
}
size_t MemoryPool::GetCertainBits(uchar bit_count, uchar start, size_t source)
{
	size_t mask = 1; // create a mask with initial of 1
	mask = (mask << bit_count) - 1; // generate enough 1's which will filter the data
	mask = mask << (32 - start - bit_count); // move the mast to the data we need
	source = source & mask; // apply the mast
	source = source >> (32 - start - bit_count); // move the bits left to the beginning for return
	return source;
}




#endif